from __future__ import print_function
import os
import argparse
import torch
import torch2trt
import torch.backends.cudnn as cudnn
import numpy as np
from data import cfg_mnet, cfg_re50
from layers.functions.prior_box import PriorBox
from utils.nms.py_cpu_nms import py_cpu_nms
import cv2
from models.retinaface import RetinaFace
from utils.box_utils import decode, decode_landm
import time

parser = argparse.ArgumentParser(description='Retinaface')

# parser.add_argument('-m', '--trained_model', default='./weights/Resnet50_Final.pth',type=str, help='Trained state_dict file path to open')
# parser.add_argument('--network', default='resnet50', help='Backbone network mobile0.25 or resnet50')

parser.add_argument('-m', '--trained_model', default='./weights/mobilenet0.25_Final.pth',type=str, help='Trained state_dict file path to open')
parser.add_argument('--network', default='mobile0.25', help='Backbone network mobile0.25 or resnet50')


parser.add_argument('--cpu', action="store_true", default=False, help='Use cpu inference')
#parser.add_argument('--confidence_threshold', default=0.6, type=float, help='confidence_threshold')
parser.add_argument('--confidence_threshold', default=0.85, type=float, help='confidence_threshold')
#parser.add_argument('--top_k', default=5000, type=int, help='top_k')
parser.add_argument('--top_k', default=2, type=int, help='top_k')
parser.add_argument('--nms_threshold', default=0.4, type=float, help='nms_threshold')
parser.add_argument('--keep_top_k', default=750, type=int, help='keep_top_k')
parser.add_argument('-s', '--save_image', action="store_true", default=True, help='show detection results')
parser.add_argument('--vis_thres', default=0.6, type=float, help='visualization_threshold')
args = parser.parse_args()


def check_keys(model, pretrained_state_dict):
    ckpt_keys = set(pretrained_state_dict.keys())
    model_keys = set(model.state_dict().keys())
    used_pretrained_keys = model_keys & ckpt_keys
    unused_pretrained_keys = ckpt_keys - model_keys
    missing_keys = model_keys - ckpt_keys
    print('Missing keys:{}'.format(len(missing_keys)))
    print('Unused checkpoint keys:{}'.format(len(unused_pretrained_keys)))
    print('Used keys:{}'.format(len(used_pretrained_keys)))
    assert len(used_pretrained_keys) > 0, 'load NONE from pretrained checkpoint'
    return True


def remove_prefix(state_dict, prefix):
    ''' Old style model is stored with all names of parameters sharing common prefix 'module.' '''
    print('remove prefix \'{}\''.format(prefix))
    f = lambda x: x.split(prefix, 1)[-1] if x.startswith(prefix) else x
    return {f(key): value for key, value in state_dict.items()}


def load_model(model, pretrained_path, load_to_cpu):
    print('Loading pretrained model from {}'.format(pretrained_path))
    if load_to_cpu:
        pretrained_dict = torch.load(pretrained_path, map_location=lambda storage, loc: storage)
    else:
        device = torch.cuda.current_device()
        pretrained_dict = torch.load(pretrained_path, map_location=lambda storage, loc: storage.cuda(device))
    if "state_dict" in pretrained_dict.keys():
        pretrained_dict = remove_prefix(pretrained_dict['state_dict'], 'module.')
    else:
        pretrained_dict = remove_prefix(pretrained_dict, 'module.')
    check_keys(model, pretrained_dict)
    model.load_state_dict(pretrained_dict, strict=False)
    return model


class VideoRecoder:
    def __init__(self,img,fn_video):
        im_height, im_width, _ = img.shape
        fourcc = cv2.VideoWriter_fourcc(*'XVID')
        self.wr = cv2.VideoWriter(fn_video,fourcc, 25.0, (im_width,im_height),True)
    def feed(self,img):
        self.wr.write(img)


class FaceDetectAPI:
    def __init__(self,img_raw):
        torch.set_grad_enabled(False)
        self.cfg = cfg_mnet
#         cfg = cfg_re50
        
        # net and model
        net = RetinaFace(cfg=self.cfg, phase = 'test')
        net = load_model(net, args.trained_model, args.cpu)
        net.eval()
#         print('Finished loading model!')
#         print(net)
        cudnn.benchmark = True
        self.device = torch.device("cpu" if args.cpu else "cuda")
        self.net = net.to(self.device)
        
        im_height, im_width, _ = img_raw.shape
        priorbox = PriorBox(self.cfg, image_size=(im_height, im_width))
        priors = priorbox.forward()
        priors = priors.to(self.device)
        self.prior_data = priors.data #         
#         vc=cv2.VideoCapture("rtsp://admin:qd123456@10.39.245.166:554/Streaming/Channels/1")
#         resize = 1    

        # transform model from pytorch to TensorRT
        """img = np.float32(img_raw)
        im_height, im_width, _ = img.shape
        scale = torch.Tensor([img.shape[1], img.shape[0], img.shape[1], img.shape[0]])
        img -= (104, 117, 123)
        img = img.transpose(2, 0, 1)
        img = torch.from_numpy(img).unsqueeze(0)
        img = img.to(self.device)
        model_trt = torch2trt.torch2trt(net, [img])
        model_trt.eval()"""
        
        
    def Detect(self,img, configParams):        
        img = np.float32(img)
        im_height, im_width, _ = img.shape
        scale = torch.Tensor([img.shape[1], img.shape[0], img.shape[1], img.shape[0]])
        img -= (104, 117, 123)
        img = img.transpose(2, 0, 1)
        img = torch.from_numpy(img).unsqueeze(0)
        img = img.to(self.device)
        scale = scale.to(self.device)
        
        loc, conf, landms = self.net(img)  # forward pass
        #loc, conf, landms = self.model_trt(img)  # forward pass
#         print('net forward time: {:.4f}'.format(time.time() - tic))

        boxes = decode(loc.data.squeeze(0), self.prior_data, self.cfg['variance'])
        boxes = boxes * scale
        boxes = boxes.cpu().numpy()
        scores = conf.squeeze(0).data.cpu().numpy()[:, 1]
        landms = decode_landm(landms.data.squeeze(0), self.prior_data, self.cfg['variance'])
        scale1 = torch.Tensor([img.shape[3], img.shape[2], img.shape[3], img.shape[2],
                               img.shape[3], img.shape[2], img.shape[3], img.shape[2],
                               img.shape[3], img.shape[2]])
        scale1 = scale1.to(self.device)
        landms = landms * scale1
        landms = landms.cpu().numpy()

        # ignore low scores
        #inds = np.where(scores > args.confidence_threshold)[0]
        inds = np.where(scores > configParams.config_faceDetectThreshold)[0]
        boxes = boxes[inds]
        landms = landms[inds]
        scores = scores[inds]

        # keep top-K before NMS
        order = scores.argsort()[::-1][:args.top_k]
        boxes = boxes[order]
        landms = landms[order]
        scores = scores[order]

        # do NMS
        dets = np.hstack((boxes, scores[:, np.newaxis])).astype(np.float32, copy=False)
        keep = py_cpu_nms(dets, args.nms_threshold)
        # keep = nms(dets, args.nms_threshold,force_cpu=args.cpu)
        dets = dets[keep, :]
        landms = landms[keep]

        # keep top-K faster NMS
        dets = dets[:args.keep_top_k, :]
        landms = landms[:args.keep_top_k, :]
        dets = np.concatenate((dets, landms), axis=1)
        
        list_face=[]
        list_landmarks=[]
        for b in dets:
            face_dict={}            
            face_dict['conf']=b[4]
            b = list(map(int, b))
            face_dict['bbox']=[b[0], b[1], b[2], b[3]]
            face_dict['eyer']=(b[5], b[6])
            face_dict['eyel']=(b[7], b[8])
            face_dict['nose']=(b[9], b[10])
            face_dict['mouthr']=(b[11], b[12])
            face_dict['mouthl']=(b[13], b[14])
            face_dict['landmarks']=[b[5],b[7],b[9],b[11],b[13],b[6],b[8],b[10],b[12],b[14]]
            list_face.append(face_dict)
            list_landmarks.append(face_dict['landmarks'])
        return list_face,list_landmarks

    def DrawFace(self,img_raw,face_dict):
        b=face_dict['bbox']
        cv2.rectangle(img_raw, (b[0], b[1]), (b[2], b[3]), (0, 0, 255), 2)
        cx = b[0]
        cy = b[1] + 12
#         cv2.putText(img_raw, text, (cx, cy),cv2.FONT_HERSHEY_DUPLEX, 0.5, (255, 255, 255))

        # landms  youyan zuoyan bizi youzui zuozui
        cv2.circle(img_raw, face_dict['eyer'], 1, (0, 0, 255), 4)
        cv2.circle(img_raw, face_dict['eyel'], 1, (0, 255, 255), 4)
        cv2.circle(img_raw, face_dict['nose'], 1, (255, 0, 255), 4)
        cv2.circle(img_raw, face_dict['mouthr'], 1, (0, 255, 0), 4)
        cv2.circle(img_raw, face_dict['mouthl'], 1, (255, 0, 0), 4)
        return img_raw

    def DrawFaces(self,img,list_face):
        img_dis=img.copy()
        for dict_face in list_face:
            img_dis=self.DrawFace(img_dis, dict_face)
        return img_dis


if __name__ == '__main__':
    
    src="/home/guo/eng/Face/Pytorch_Retinaface/faceVideo3.avi"
#     src="rtsp://admin:qd123456@10.39.245.166:554/Streaming/Channels/1"
    vc=cv2.VideoCapture(0)
    res,img_raw=vc.read()
    vr=VideoRecoder(img_raw,"faceVideo_0.avi")
    fd=FaceDetectAPI(img_raw)
    # testing begin
    counter=0
    while True:
        face_flag=False
        image_path = "./curve/test.jpg"
#         img_raw = cv2.imread(image_path, cv2.IMREAD_COLOR)
        res,img_raw=vc.read()
        
        if res==False:
            continue
        img_write=img_raw.copy()
        counter+=1
        if counter%1000==0:
            print(counter)
        list_face,list_landmarks=fd.Detect(img_raw)       
        img_dis=fd.DrawFaces(img_raw, list_face)
        vr.feed(img_write)
        cv2.imshow("img_dis",img_dis)
        cv2.waitKey(1)
